#include "arm_control/arm_action_type10.hpp"
#include <chrono>
#include <thread>
#include <algorithm>

// B右臂抓取并放置动作
// 订阅/detect_right话题，根据offset调整xuan12和yuntai2关节，
// 进行运动规划后x轴正向移动10cm，然后抓取并放置

namespace arm_control
{

ArmActionType10::ArmActionType10(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 创建订阅者
  right_sub_ = node_->create_subscription<fruit_detector::msg::DetectionArray>(
    "/detect_right", 10,
    std::bind(&ArmActionType10::right_callback, this, std::placeholders::_1));
  
  // 创建爪子发布器
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
    
  // 初始化 MoveIt 接口
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  // 创建 VoicePlanner action 客户端
  voice_planner_client_ = rclcpp_action::create_client<VoicePlanner>(node_, "voice_planner");
  
  RCLCPP_INFO(logger_, "动作类型10已创建");
  RCLCPP_INFO(logger_, "订阅话题: /detect_right");
  RCLCPP_INFO(logger_, "创建爪子发布器: /zhuazi_right");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化: arm_right");
  RCLCPP_INFO(logger_, "arm_right规划参考坐标系: %s", move_group_right_->getPlanningFrame().c_str());
  RCLCPP_INFO(logger_, "VoicePlanner客户端已创建");
}

void ArmActionType10::right_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg)
{
  right_data_.clear();
  for (const auto& fruit : msg->fruits) {
    FruitData10 data;
    data.type = fruit.type;
    data.offset_x = fruit.offset_x;
    data.offset_y = fruit.offset_y;
    data.isgood = fruit.isgood;
    right_data_.push_back(data);
  }
  RCLCPP_DEBUG(logger_, "收到右侧检测数据，共 %zu 个水果", right_data_.size());
}

FruitData10 ArmActionType10::select_best_fruit(
  const std::vector<int32_t>& type_array,
  const std::vector<FruitData10>& data,
  bool& found)
{
  found = false;
  FruitData10 best_fruit;
  float min_offset = std::numeric_limits<float>::max();
  
  // 遍历所有数据
  for (const auto& fruit : data) {
    // 检查是否在类型数组中
    auto it = std::find(type_array.begin(), type_array.end(), fruit.type);
    if (it != type_array.end()) {
      // 计算offset
      float offset = fruit.get_offset();
      // 如果offset更小，则更新最佳水果
      if (offset < min_offset) {
        min_offset = offset;
        best_fruit = fruit;
        found = true;
      }
    }
  }
  
  return best_fruit;
}

bool ArmActionType10::move_with_joint_adjustment(double offset_x, double offset_y)
{
  RCLCPP_INFO(logger_, "[右臂] 开始根据offset调整关节并规划: offset_x=%.2f, offset_y=%.2f", 
              offset_x, offset_y);
  
  // 更新当前状态
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前状态
  moveit::core::RobotStatePtr current_state = move_group_right_->getCurrentState(10.0);
  if (!current_state) {
    RCLCPP_ERROR(logger_, "[右臂] 无法获取当前机器人状态");
    return false;
  }
  
  // 获取关节模型组
  const moveit::core::JointModelGroup* joint_model_group = 
    current_state->getJointModelGroup("arm_right");
  
  // 获取当前关节位置
  std::vector<double> joint_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_positions);
  
  // 获取关节名称
  const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
  
  // 打印当前关节值
  RCLCPP_INFO(logger_, "[右臂] 当前关节状态:");
  for (size_t i = 0; i < joint_names.size(); i++) {
    RCLCPP_INFO(logger_, "  %s = %.4f", joint_names[i].c_str(), joint_positions[i]);
  }
  
  // 修改xuan12和yuntai2的值
  for (size_t i = 0; i < joint_names.size(); i++) {
    if (joint_names[i] == "xuan12") {
      double original_value = joint_positions[i];
      joint_positions[i] = original_value - 0.003 * offset_x;
      RCLCPP_INFO(logger_, "[右臂] xuan12: %.4f -> %.4f (offset_x=%.2f)", 
                  original_value, joint_positions[i], offset_x);
    }
    else if (joint_names[i] == "yuntai2") {
      double original_value = joint_positions[i];
      // 根据offset_y调整yuntai2，使用类似的调整系数
      joint_positions[i] = original_value - 0.003 * offset_y;
      RCLCPP_INFO(logger_, "[右臂] yuntai2: %.4f -> %.4f (offset_y=%.2f)", 
                  original_value, joint_positions[i], offset_y);
    }
  }
  
  // 设置目标关节值
  move_group_right_->setJointValueTarget(joint_positions);
  
  // 进行运动规划
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  bool plan_success = (move_group_right_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!plan_success) {
    RCLCPP_ERROR(logger_, "[右臂] 关节调整运动规划失败");
    return false;
  }
  
  RCLCPP_INFO(logger_, "[右臂] 关节调整运动规划成功，开始执行...");
  
  // 执行运动
  auto execute_result = move_group_right_->execute(plan);
  if (execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "[右臂] 关节调整运动执行失败");
    return false;
  }
  
  RCLCPP_INFO(logger_, "[右臂] 关节调整运动执行成功");
  return true;
}

bool ArmActionType10::move_x_positive(double distance)
{
  RCLCPP_INFO(logger_, "[右臂] 开始x轴正向移动: %.3f米", distance);
  
  // 更新当前状态
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前末端位姿
  geometry_msgs::msg::PoseStamped current_pose = move_group_right_->getCurrentPose();
  RCLCPP_INFO(logger_, "[右臂] 当前末端位置: x=%.3f, y=%.3f, z=%.3f", 
              current_pose.pose.position.x,
              current_pose.pose.position.y,
              current_pose.pose.position.z);
  
  // 创建路径点（沿x轴正向移动）
  std::vector<geometry_msgs::msg::Pose> waypoints;
  geometry_msgs::msg::Pose target_pose = current_pose.pose;
  target_pose.position.x += distance;
  waypoints.push_back(target_pose);
  
  RCLCPP_INFO(logger_, "[右臂] 目标位置: x=%.3f, y=%.3f, z=%.3f", 
              target_pose.position.x,
              target_pose.position.y,
              target_pose.position.z);
  
  // 计算笛卡尔路径
  moveit_msgs::msg::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;  // 禁用跳跃检测
  const double eef_step = 0.01;       // 1cm 步长
  
  double fraction = move_group_right_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  
  RCLCPP_INFO(logger_, "[右臂] 笛卡尔路径规划完成度: %.2f%%", fraction * 100.0);
  
  if (fraction < 0.80) {  // 至少完成80%的路径
    RCLCPP_ERROR(logger_, "[右臂] 笛卡尔路径规划失败，完成度不足");
    return false;
  }
  
  // 执行轨迹
  RCLCPP_INFO(logger_, "[右臂] 开始执行x轴正向运动...");
  auto execute_result = move_group_right_->execute(trajectory);
  
  if (execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "[右臂] x轴运动执行失败");
    return false;
  }
  
  RCLCPP_INFO(logger_, "[右臂] x轴正向运动执行成功");
  return true;
}

void ArmActionType10::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型10 - 右臂抓取并放置动作");
  
  const auto goal = goal_handle->get_goal();
  
  // 获取右侧类型数组
  const auto& right_types = goal->right_type;
  
  RCLCPP_INFO(logger_, "右侧类型数组大小: %zu", right_types.size());
  
  // 等待一段时间确保已收到检测数据
  feedback->progress_percentage = 5.0;
  feedback->current_status = "等待检测数据...";
  goal_handle->publish_feedback(feedback);
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型10已取消");
    return;
  }
  
  // 检查右侧类型数组是否为空
  if (right_types.empty()) {
    RCLCPP_WARN(logger_, "右侧类型数组为空");
    result->success = false;
    result->message = "动作类型10失败 - 右侧类型数组为空";
    goal_handle->abort(result);
    return;
  }
  
  // ========== 第一步：分析右侧检测数据 ==========
  feedback->progress_percentage = 10.0;
  feedback->current_status = "分析右侧检测数据...";
  goal_handle->publish_feedback(feedback);
  
  bool right_found = false;
  FruitData10 right_fruit = select_best_fruit(right_types, right_data_, right_found);
  
  if (!right_found) {
    RCLCPP_WARN(logger_, "右侧未找到匹配的水果");
    result->success = false;
    result->message = "动作类型10失败 - 右侧未找到匹配的水果";
    goal_handle->abort(result);
    return;
  }
  
  RCLCPP_INFO(logger_, "右侧找到匹配水果: 类型=%d, offset_x=%.2f, offset_y=%.2f, offset=%.2f", 
              right_fruit.type, right_fruit.offset_x, right_fruit.offset_y, right_fruit.get_offset());
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    return;
  }
  
  /* ========== 第二步：根据offset调整关节并运动规划（已注释）==========
  feedback->progress_percentage = 20.0;
  feedback->current_status = "调整关节值并规划运动...";
  goal_handle->publish_feedback(feedback);
  
  bool adjust_success = move_with_joint_adjustment(right_fruit.offset_x, right_fruit.offset_y);
  
  if (!adjust_success) {
    RCLCPP_ERROR(logger_, "关节调整运动失败");
    result->success = false;
    result->message = "动作类型10失败 - 关节调整运动失败";
    goal_handle->abort(result);
    return;
  }
  
  RCLCPP_INFO(logger_, "关节调整运动完成");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    return;
  }
  ========== 关节调整步骤注释结束 ========== */
  
  // ========== 第二步：调用voice_planner类型4动作 ==========
  feedback->progress_percentage = 50.0;
  feedback->current_status = "执行voice_planner类型4动作...";
  goal_handle->publish_feedback(feedback);
  
  // 根据水果类型确定参数
  // 苹果（type=0或1）-> 参数1
  // 梨子（type=2或3）-> 参数2
  int32_t voice_param = 0;
  if (right_fruit.type == 0 || right_fruit.type == 1) {
    voice_param = 1;
    RCLCPP_INFO(logger_, "检测到苹果（类型=%d），voice_planner参数设置为1", right_fruit.type);
  } else if (right_fruit.type == 2 || right_fruit.type == 3) {
    voice_param = 2;
    RCLCPP_INFO(logger_, "检测到梨子（类型=%d），voice_planner参数设置为2", right_fruit.type);
  } else {
    RCLCPP_WARN(logger_, "未知水果类型=%d，默认设置参数为1", right_fruit.type);
    voice_param = 1;
  }
  
  // 等待voice_planner服务可用
  if (!voice_planner_client_->wait_for_action_server(std::chrono::seconds(5))) {
    RCLCPP_ERROR(logger_, "VoicePlanner action服务器不可用");
    result->success = false;
    result->message = "动作类型10失败 - VoicePlanner服务器不可用";
    goal_handle->abort(result);
    return;
  }
  
  // 创建voice_planner目标
  auto voice_goal = VoicePlanner::Goal();
  voice_goal.type = 4;
  voice_goal.data.push_back(voice_param);
  
  RCLCPP_INFO(logger_, "发送voice_planner请求: type=4, data[0]=%d", voice_param);
  
  // 发送goal并等待结果
  auto voice_goal_handle_future = voice_planner_client_->async_send_goal(voice_goal);
  
  // 等待goal被接受
  if (rclcpp::spin_until_future_complete(node_, voice_goal_handle_future, std::chrono::seconds(5)) !=
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "发送voice_planner goal失败");
    result->success = false;
    result->message = "动作类型10失败 - 发送voice_planner goal失败";
    goal_handle->abort(result);
    return;
  }
  
  auto voice_goal_handle = voice_goal_handle_future.get();
  if (!voice_goal_handle) {
    RCLCPP_ERROR(logger_, "voice_planner goal被拒绝");
    result->success = false;
    result->message = "动作类型10失败 - voice_planner goal被拒绝";
    goal_handle->abort(result);
    return;
  }
  
  // 等待结果
  auto voice_result_future = voice_planner_client_->async_get_result(voice_goal_handle);
  if (rclcpp::spin_until_future_complete(node_, voice_result_future, std::chrono::seconds(10)) !=
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "获取voice_planner结果超时");
    result->success = false;
    result->message = "动作类型10失败 - voice_planner执行超时";
    goal_handle->abort(result);
    return;
  }
  
  auto voice_result = voice_result_future.get();
  RCLCPP_INFO(logger_, "voice_planner执行完成: %s", voice_result.result->message.c_str());
  
  if (!voice_result.result->success) {
    RCLCPP_ERROR(logger_, "voice_planner执行失败");
    result->success = false;
    result->message = "动作类型10失败 - voice_planner执行失败";
    goal_handle->abort(result);
    return;
  }
  
  /* ========== 以下为暂时注释的机械臂抓取和放置动作 ==========
  
  // ========== 第三步：x轴正向移动10cm ==========
  feedback->progress_percentage = 40.0;
  feedback->current_status = "x轴正向移动10cm...";
  goal_handle->publish_feedback(feedback);
  
  bool move_x_success = move_x_positive(0.10);  // 10cm = 0.10m
  
  if (!move_x_success) {
    RCLCPP_ERROR(logger_, "x轴正向移动失败");
    result->success = false;
    result->message = "动作类型10失败 - x轴正向移动失败";
    goal_handle->abort(result);
    return;
  }
  
  RCLCPP_INFO(logger_, "x轴正向移动完成");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    return;
  }
  
  // ========== 第四步：关闭爪子（抓取）==========
  feedback->progress_percentage = 60.0;
  feedback->current_status = "关闭爪子抓取...";
  goal_handle->publish_feedback(feedback);
  
  auto grasp_msg = std_msgs::msg::Int32();
  grasp_msg.data = 0;
  zhuazi_right_publisher_->publish(grasp_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 0 (关闭抓取)");
  
  // 等待1.5秒让爪子动作完成
  RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    return;
  }
  
  // ========== 第五步：设置yuntai2为0，xuan12为2π并运动规划 ==========
  feedback->progress_percentage = 70.0;
  feedback->current_status = "移动到放置位置...";
  goal_handle->publish_feedback(feedback);
  
  RCLCPP_INFO(logger_, "设置yuntai2为0，xuan12为2π");
  
  // 更新当前状态
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前状态
  moveit::core::RobotStatePtr current_state = move_group_right_->getCurrentState(10.0);
  if (!current_state) {
    RCLCPP_ERROR(logger_, "无法获取当前机器人状态");
    result->success = false;
    result->message = "动作类型10失败 - 无法获取当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取关节模型组
  const moveit::core::JointModelGroup* joint_model_group = 
    current_state->getJointModelGroup("arm_right");
  
  // 获取当前关节位置
  std::vector<double> joint_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_positions);
  
  // 获取关节名称
  const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
  
  // 修改yuntai2和xuan12的值
  for (size_t i = 0; i < joint_names.size(); i++) {
    if (joint_names[i] == "yuntai2") {
      joint_positions[i] = 0.0;
      RCLCPP_INFO(logger_, "设置yuntai2为0.0");
    }
    else if (joint_names[i] == "xuan12") {
      joint_positions[i] = 2.0 * M_PI;  // 2π
      RCLCPP_INFO(logger_, "设置xuan12为2π (%.4f)", joint_positions[i]);
    }
  }
  
  // 设置目标关节值
  move_group_right_->setJointValueTarget(joint_positions);
  
  // 进行运动规划
  moveit::planning_interface::MoveGroupInterface::Plan release_plan;
  bool release_plan_success = (move_group_right_->plan(release_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!release_plan_success) {
    RCLCPP_ERROR(logger_, "放置位置运动规划失败");
    result->success = false;
    result->message = "动作类型10失败 - 放置位置运动规划失败";
    goal_handle->abort(result);
    return;
  }
  
  RCLCPP_INFO(logger_, "放置位置运动规划成功，开始执行...");
  
  // 执行运动
  auto release_execute_result = move_group_right_->execute(release_plan);
  if (release_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "放置位置运动执行失败");
    result->success = false;
    result->message = "动作类型10失败 - 放置位置运动执行失败";
    goal_handle->abort(result);
    return;
  }
  
  RCLCPP_INFO(logger_, "已到达放置位置");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型10已取消";
    goal_handle->canceled(result);
    return;
  }
  
  // ========== 第六步：释放爪子 ==========
  feedback->progress_percentage = 90.0;
  feedback->current_status = "释放爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto release_msg = std_msgs::msg::Int32();
  release_msg.data = 120;
  zhuazi_right_publisher_->publish(release_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (打开释放)");
  
  // 等待爪子动作完成
  std::this_thread::sleep_for(std::chrono::milliseconds(1500));
  
  ========== 机械臂抓取和放置动作注释结束 ========== */
  
  // ========== 完成 ==========
  feedback->progress_percentage = 100.0;
  feedback->current_status = "动作完成";
  goal_handle->publish_feedback(feedback);
  
  result->success = true;
  result->message = "动作类型10执行成功 - 右臂抓取并放置完成 (类型=" + 
                    std::to_string(right_fruit.type) + 
                    ", offset=" + std::to_string(right_fruit.get_offset()) + ")";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型10执行成功");
}

} // namespace arm_control

